#include"esikf_slam/pc_handle.h"

namespace esikf_slam{

void PCHandle::preHandle(const livox_ros_driver::CustomMsg::ConstPtr &msg,pcl::PointCloud<PointType>::Ptr& pc_ret){
    pc_surface_.reset(new pcl::PointCloud<PointType>);
    // pc_conner_.reset(new pcl::PointCloud<PointType>);
    pc_full_.reset(new pcl::PointCloud<PointType>);
    pc_full_->reserve(msg->point_num);
    // pc_conner_->reserve(msg->point_num);
    pc_surface_->reserve(msg->point_num);

    // 等间隔降采样
    int valid_num = 0;
    for (uint i = 1; i < msg->point_num; i++)
    {
        if ((msg->points[i].line < p_config_->n_scan_) && ((msg->points[i].tag & 0x30) == 0x10 || (msg->points[i].tag & 0x30) == 0x00))
        {
            valid_num++;
            if (valid_num % p_config_->point_filter_num_ == 0)
            {
                pc_full_->points[i].x = msg->points[i].x;
                pc_full_->points[i].y = msg->points[i].y;
                pc_full_->points[i].z = msg->points[i].z;
                pc_full_->points[i].intensity = msg->points[i].reflectivity;
                pc_full_->points[i].curvature = double(msg->points[i].offset_time *(0.000000001)); //use curvature as time of each laser points

                if ((abs(pc_full_->points[i].x - pc_full_->points[i - 1].x) > 1e-7) || 
                    (abs(pc_full_->points[i].y - pc_full_->points[i - 1].y) > 1e-7) || 
                    (abs(pc_full_->points[i].z - pc_full_->points[i - 1].z) > 1e-7) && 
                    (pc_full_->points[i].x * pc_full_->points[i].x + pc_full_->points[i].y * pc_full_->points[i].y > p_config_->range_threshold_))
                {
                    pc_surface_->push_back(pc_full_->points[i]);
                }
            }
        }
    }
    pc_ret = pc_surface_;
}



}
